#include "stdafx.h"

#include "GDIUtils.h"
#include "Polygon.h"
#include "Spline.h"

//#define		DIV_FACTOR	4.0			//adjust this factor to adjust the curve smoothness
extern	double			g_fSplineSampleError;	// the sampling error for spline curve


////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
//		 class CCurve
////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////

CCurve::CCurve(double ax,double ay,double bx,double by,double cx,double cy,int ndiv): m_XOrg(0), m_YOrg(0)
{
	Set(ax, ay, bx, by, cx, cy );
	Ndiv = ndiv;
}


CCurve::CCurve(double ax,double ay,double bx,double by,double cx,double cy): m_XOrg(0), m_YOrg(0)
{
	Set(ax, ay, bx, by, cx, cy );
}

CCurve::CCurve(): m_XOrg(0), m_YOrg(0)
{	
	Set(0,0,0,0,0,0);
};

void	CCurve::Set(double ax,double ay,double bx,double by,double cx,double cy) 
{
	Ax = ax;
	Ay = ay;
	Bx = bx; 
	By = by;
	Cx = cx; 
	Cy = cy;
	//Ndiv = (int)(max(abs((int)Ax),abs((int)Ay))/g_fSplineSampleError);	// default value

	double length = 0;
	CVertex prev( X(0), Y(0) );
	for( int i=1; i<=100; i++ )
	{
		double t =(double)i/100.0;
		CVertex curv( X(t), Y(t) );
		length += CGDIUtils::Distance( prev, curv );
		prev = curv;
	}
	Ndiv = (int)(length/g_fSplineSampleError );
}

// number of points on the curve
int		CCurve::NumPoints()
{
	if (Ndiv==0)
		Ndiv=1;

	return Ndiv+1;
}

double	CCurve::X( double t )
{
	double f = t*t*(3.0f-2.0f*t);
	double g = t*(t-1.0f)*(t-1.0f);
	double h = t*t*(t-1.0f);
	double X = m_XOrg + Ax*f + Bx*g + Cx*h;

	return X;
}

double	CCurve::Y( double t )
{
	double f = t*t*(3.0f-2.0f*t);
	double g = t*(t-1.0f)*(t-1.0f);
	double h = t*t*(t-1.0f);
	double Y = m_YOrg + Ay*f + By*g + Cy*h;

	return Y;
}

// get the points of the curve
void	CCurve::GetPoints( double x, double y, std::vector<CVertex> &points )
{
	points.clear();

	SetOrg( x, y );

	if (Ndiv==0)
		Ndiv=1;

	points.push_back( CVertex(x,y) );
	for(int i=1; i<=Ndiv ; i++)
	{
		double t = 1.0f / (double)Ndiv * (double)i;
		points.push_back( CVertex(X(t),Y(t)) );
	}
}

// get the points of the curve
void	CCurve::GetPointsAdaptively( double x, double y, std::vector<CVertex> &points, double fError )
{
}

// draw the curve segment
void	CCurve::Draw( CDC *pDC, double x, double y )
{
	std::vector<CVertex>	points;
	GetPoints( x, y, points );
	assert( points.size() > 1 );

	CVertex &pV = points[0];
	pDC->MoveTo( (int)pV.x, (int)pV.y );

	for( int i=1; i<(int)points.size(); i++ )
	{
		pV = points[i];
		pDC->LineTo( (int)pV.x, (int)pV.y );
	}
}

void	CCurve::DrawInGL(double x, double y,
						 CSize window_size, float fGLViewWidth, float fGLViewHeight, 
						  CPseudoColorRGB &PsdC,Vector2 Origin/*=Vector2(0,0)*/, 
						  bool bDrawCurve/*=true*/, bool bDrawVertex/*=true*/)
{
	std::vector<CVertex>	points;
	GetPoints( x, y, points );
	assert( points.size() > 1 );
	GLubyte PsdColor[3]= {255, 255, 255};
	PsdC.SetPCValueRange(0.0, 1.0); 
// 	float abs_mean,abs_max;
// 	abs_max = points.size();

	float sz1[2] ={window_size.cx*0.5,window_size.cy*0.5};
	float sz2[2] ={2.0*fGLViewWidth/window_size.cx, 2.0*fGLViewHeight/window_size.cy};
	if(bDrawCurve)
	{	
		glColor3f(1.0,0.2,0.1);
		glBegin(GL_LINE_STRIP);
		CVertex &pV = points[0];	
		glVertex2f( (pV.x+Origin.x-sz1[0])*sz2[0], ( sz1[1]- (pV.y+Origin.y) )*sz2[1]);
		for( int i=1; i<(int)points.size(); i++ )
		{
			pV = points[i];
			glVertex2f( (pV.x+Origin.x-sz1[0])*sz2[0], ( sz1[1]- (pV.y+Origin.y) )*sz2[1]);		
		}
		glEnd();		
	}
	if(bDrawVertex){
		glColor3f(0,0.25,0.25);
		glPointSize(4.0);
		glBegin(GL_POINTS);
		for( int i=1; i<(int)points.size()-1; i+=3 )	
			glVertex2f( ( points[i].x+Origin.x-sz1[0])*sz2[0], ( sz1[1]- ( points[i].y+Origin.y) )*sz2[1]);		
		glEnd();	
	}
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
//		 class CSpline
////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////

// constructor
CSpline::CSpline(POINT pt[], int np)
{
	Init();
	SetSize(np);

	for(int i=0;i<NP ;i++) 
	{
		Px[i] = (double)pt[i].x;  
		Py[i] = (double)pt[i].y;
	}
}

CSpline::CSpline(double px[] , double py[] , int np) 
{
	Init();
	SetSize( np );
	for(int i=0;i<NP ;i++) 
	{
		Px[i] = px[i];  
		Py[i] = py[i];
	}
}

CSpline::CSpline( CPolygon *pPoly )
{
	Init();
	int num = pPoly->NumVertex();
	SetSize(num);
	for( int i=0; i<num; i++ )
	{
		CVertex *pV = pPoly->GetVertex(i);
		Px[i] = pV->x;
		Py[i] = pV->y;
	}
}

CSpline::~CSpline()
{
	Clear();
}

void	CSpline::Init()
{
	Px = NULL;
	Py = NULL;
	Ax = NULL;
	Ay = NULL;
	Bx = NULL;
	By = NULL;
	Cx = NULL;
	Cy = NULL;
	k = NULL;
	Mat[0] = NULL;
	Mat[1] = NULL;
	Mat[2] = NULL;
}

void	CSpline::SetSize( int np )
{
	Clear();

	NP = np;
	Px = new double[NP];
	Py = new double[NP];
	Ax = new double[NP];
	Ay = new double[NP];
	Bx = new double[NP];
	By = new double[NP];
	Cx = new double[NP];
	Cy = new double[NP];
	k = new double[NP];
	Mat[0] = new double[NP];
	Mat[1] = new double[NP];
	Mat[2] = new double[NP];
}

void	CSpline::Clear()
{
	if( Px )	delete[] Px;
	if( Py )	delete[] Py;
	if( Ax )	delete[] Ax;
	if( Ay )	delete[] Ay;
	if( Bx )	delete[] Bx;
	if( By )	delete[] By;
	if( Cx )	delete[] Cx;
	if( Cy )	delete[] Cy;
	if( k )		delete[] k;
	if(Mat[0])	delete[] Mat[0];
	if(Mat[1])	delete[] Mat[1];
	if(Mat[2])	delete[] Mat[2];
}

void	CSpline::Generate() 
{
	double AMag , AMagOld;
    // vector A
	for(int i= 0 ; i<=NP-2 ; i++ ) 
	{
		Ax[i] = Px[i+1] - Px[i];
		Ay[i] = Py[i+1] - Py[i];
	}
	// k
	AMagOld = sqrt(Ax[0]*Ax[0] + Ay[0]*Ay[0]);
	for(int i=0 ; i<=NP-3 ; i++) 
	{
		AMag = sqrt(Ax[i+1]*Ax[i+1] + Ay[i+1]*Ay[i+1]);
		k[i] = AMagOld / AMag;
		AMagOld = AMag;
	}
	k[NP-2] = 1.0f;

	// Matrix
	for(int i=1; i<=NP-2;i++) 
	{
		Mat[0][i] = 1.0f;
		Mat[1][i] = 2.0f*k[i-1]*(1.0f + k[i-1]);
		Mat[2][i] = k[i-1]*k[i-1]*k[i];
	}
	Mat[1][0] = 2.0f;
	Mat[2][0] = k[0];
	Mat[0][NP-1] = 1.0f;
	Mat[1][NP-1] = 2.0f*k[NP-2];

	// 
	for(int i=1; i<=NP-2;i++) 
	{
		Bx[i] = 3.0f*(Ax[i-1] + k[i-1]*k[i-1]*Ax[i]);
		By[i] = 3.0f*(Ay[i-1] + k[i-1]*k[i-1]*Ay[i]);
	}
	Bx[0] = 3.0f*Ax[0];
	By[0] = 3.0f*Ay[0];
	Bx[NP-1] = 3.0f*Ax[NP-2];
	By[NP-1] = 3.0f*Ay[NP-2];

	//
	MatrixSolve(Bx);
	MatrixSolve(By);

	for(int i=0 ; i<=NP-2 ; i++ ) 
	{
		Cx[i] = k[i]*Bx[i+1];
		Cy[i] = k[i]*By[i+1];
	}
}

void	CSpline::MatrixSolve(double B[]) 
{
	double* Work = new double[NP];
	double* WorkB = new double[NP];
	for(int i=0;i<=NP-1;i++) 
	{
		Work[i] = B[i] / Mat[1][i];
		WorkB[i] = Work[i];
	}

	for(int j=0 ; j<10 ; j++) 
	{ ///  need convergence judge
		Work[0] = (B[0] - Mat[2][0]*WorkB[1])/Mat[1][0];
		for(int i=1; i<NP-1 ; i++ ) 
		{
			Work[i] = (B[i]-Mat[0][i]*WorkB[i-1]-Mat[2][i]*WorkB[i+1])
						/Mat[1][i];
		}
		Work[NP-1] = (B[NP-1] - Mat[0][NP-1]*WorkB[NP-2])/Mat[1][NP-1];

		for(int i=0 ; i<=NP-1 ; i++ ) 
		{
			WorkB[i] = Work[i];
		}
	}
	for(int i=0 ; i<=NP-1 ; i++ ) 
	{
		B[i] = Work[i];
	}
	delete[] Work;
	delete[] WorkB;
}


int		CSpline::NumPoints()
{
	CCurve c;
	int count = 0;
	for(int i=0; i<NP-1 ; i++) 
	{
		c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
		count += (c.NumPoints()-1);
	}

	return count+1;
}

void  CSpline::GetPoints( std::vector<CVertex> &verts, bool closed )
{
	verts.clear();

	// NP-1 curve segments
	if( !closed )	// non-closed spline
	{
		for(int i=0; i<NP-1 ; i++) 
		{
			CCurve c;
			c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
			std::vector<CVertex> vts;
			c.GetPoints( Px[i] ,Py[i], vts );
			for( int j=0; j<(int)vts.size()-1; j++ )
			{
				verts.push_back( vts[j] );
			}

			if( i==NP-2 )
			{
				verts.push_back( vts[ vts.size()-1 ] );
			}
		}
	}
	else		// closed spline: add one more curve segment to the end
	{
		for(int i=0; i<NP ; i++) 
		{
			CCurve c;
			std::vector<CVertex> vts;
			if( i<NP-1 )
			{
				c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
			}
			else	// the last segment
			{
				assert( i==NP-1 );
				c.Set(Px[0]-Px[i],Py[0]-Py[i],Cx[i-1],Cy[i-1],Bx[0],By[0]);
			}
			c.GetPoints( Px[i] ,Py[i], vts );

			for( int j=0; j<(int)vts.size()-1; j++ )
			{
				verts.push_back( vts[j] );
			}

			//if( i==NP-1 )
			//{
			//	verts.push_back( vts[ vts.size()-1 ] );
			//}
		}
	}
}


void	CSpline::Draw( CDC *pDC ) 
{
	CCurve c;
	for(int i=0; i<NP-1 ; i++) 
	{
		c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
		c.Draw( pDC,Px[i],Py[i] );
	}
}


void	CSpline::DrawInGL( CSize window_size, float fGLViewWidth, float fGLViewHeight, 						 
						 CPseudoColorRGB &PsdC,Vector2 Origin/*=Vector2(0,0)*/,
						 bool bDrawCurve/*=true*/, bool bDrawVertex/*=true*/)
{
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	glEnable(GL_LINE_SMOOTH);
	glDisable(GL_BLEND);
	glDisable(GL_TEXTURE_2D);
	glDisable(GL_LIGHTING);
	glDisable(GL_CULL_FACE);
	glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
	glLineWidth(5.0);
	glPushMatrix();
	CCurve c;
	for(int i=0; i<NP-1 ; i++) 
	{
		c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
		c.DrawInGL(Px[i],Py[i],window_size,fGLViewWidth,fGLViewHeight,PsdC,Origin,bDrawCurve,bDrawVertex);
	}	
	glPopMatrix();
	glPopAttrib();
}

//////////// closed cubic spline ////////////////////
// have bugs...
void CSpline::GenClosed() 
{
	double AMag , AMagOld , AMag0;
    // vector A
	for(int i= 0 ; i<=NP-2 ; i++ ) 
	{
		Ax[i] = Px[i+1] - Px[i];
		Ay[i] = Py[i+1] - Py[i];
	}
	Ax[NP-1] = Px[0] - Px[NP-1];
	Ay[NP-1] = Py[0] - Py[NP-1];

	// k
	AMag0 = AMagOld = sqrt(Ax[0]*Ax[0] + Ay[0]*Ay[0]);
	for(int i=0 ; i<=NP-2 ; i++) 
	{
		AMag = sqrt(Ax[i+1]*Ax[i+1] + Ay[i+1]*Ay[i+1]);
		k[i] = AMagOld / AMag;
		AMagOld = AMag;
	}
	k[NP-1]=AMagOld/AMag0; 

	// Matrix
	for(int i=0; i<=NP-1;i++) 
	{
		Mat[0][i] = 1.0f;
		Mat[1][1] = 2.0f*k[i-1]*(1.0f + k[i-1]);
		Mat[2][i] = k[i-1]*k[i-1]*k[i];
	}
	Mat[0][0] = 1.0f;
	Mat[1][0] = 2.0f*k[NP-1]*(1.0f + k[NP-1]);
	Mat[2][0] = k[NP-1]*k[NP-1]*k[0];

	// 
	for(int i=0; i<=NP-1;i++) 
	{
		Bx[i] = 3.0f*(Ax[i-1] + k[i-1]*k[i-1]*Ax[i]);
		By[i] = 3.0f*(Ay[i-1] + k[i-1]*k[i-1]*Ay[i]);
	}
	Bx[0] = 3.0f*(Ax[NP-1] + k[NP-1]*k[NP-1]*Ax[0]);
	By[0] = 3.0f*(Ay[NP-1] + k[NP-1]*k[NP-1]*Ay[0]);

	//
	MatrixSolveEX(Bx);
	MatrixSolveEX(By);

	for(int i=0 ; i<=NP-2 ; i++ ) 
	{
		Cx[i] = k[i]*Bx[i+1];
		Cy[i] = k[i]*By[i+1];
	}
	Cx[NP-1] = k[NP-1]*Bx[0];
	Cy[NP-1] = k[NP-1]*By[0];
}

///// tridiagonal matrix + elements of [0][0], [N-1][N-1] //// 
void CSpline::MatrixSolveEX(double B[]) 
{
	double* Work = new double[NP];
	double* WorkB = new double[NP];

	for(int i=0;i<=NP-1;i++) 
	{
		Work[i] = B[i] / Mat[1][i];
		WorkB[i] = Work[i];
	}

	for(int j=0 ; j<10 ; j++) 
	{  // need judge of convergence
		Work[0] = (B[0]-Mat[0][0]*WorkB[NP-1]-Mat[2][0]*WorkB[1])
				/Mat[1][0];
		for(int i=1; i<NP-1 ; i++ ) 
		{
			Work[i] = (B[i]-Mat[0][i]*WorkB[i-1]-Mat[2][i]*WorkB[i+1])
						/Mat[1][i];
		}
		Work[NP-1] = (B[NP-1]-Mat[0][NP-1]*WorkB[NP-2]-Mat[2][NP-1]*WorkB[0])
						/Mat[1][NP-1];

		for(int i=0 ; i<=NP-1 ; i++ ) 
		{
			WorkB[i] = Work[i];
		}
	}

	for(int i=0 ; i<=NP-1 ; i++ ) 
	{
		B[i] = Work[i];
	}
}

void CSpline::DrawClosed( CDC *pDC ) 
{
	CCurve c;
	for(int i=0; i<NP ; i++) 
	{
		c.Set(Ax[i],Ay[i],Bx[i],By[i],Cx[i],Cy[i]);
		c.Draw( pDC ,Px[i],Py[i] );
	}
}

void CSpline::VertexNormal()
{
	Normal.clear();	Normal.resize(NP);
	Normal[0] = Vector2(-(Py[1] - Py[0]) ,Px[1] - Px[0]);
	for (int i=1; i<NP-1; ++i){
		Normal[i] = Vector2(-(Py[i+1] - Py[i-1]) ,Px[i+1] - Px[i-1]);
	}
	Normal[NP-1] = Vector2(-(Py[NP-1] - Py[NP-2]) , (Px[NP-1] - Px[NP-2]));

	for (int i=0; i<NP; ++i){
		Normal[i].normalize();
	}
}

void CSpline::DrawNormal(CDC *pDC)
{
	for (int i=0; i<NP; ++i){
		pDC->MoveTo( (int)Px[i], (int)Py[i] );
		pDC->LineTo( (int)(Px[i]+Normal[i].x), (int)(Py[i]+Normal[i].y));
	}
}